% FindDipolarity.m
%
% This script takes an input Bz map, asks for a box, fits a source as a
% dipole, and computes the dipolarity parameter as defined in:
%
% R.R. Fu, E.A. Lima, M.W.R. Volk, R. Trubko (2020) "High-sensitivity
% moment magnetometry with the quantum diamond microscope,"  Geochemistry,
% Geophysics, Geosystems.

% -----------------------------------------------------------------------------------
%                Matlab code by Roger R. Fu and Eduardo A. Lim
%      Copyright (C) 2020 Harvard Paleomagnetics Lab, MIT Paleomagnetism Lab
% -----------------------------------------------------------------------------------

% Run this script in Matlab to bring up interactive prompt to choose input
% map and to select region of interest

% Adjust how many iterations the code undertakes to fit the dipole.  If the
% output image of the best-fit dipole is consistent with the map, there is
% no need to adjust this parameter
NRUNS=5;

% number of points used in determining the optimal solution
MINTOL=1;     

% rough estimate of order of magnitude of the moment in Am^2 to adjust converge criteria
m0=1e-11;       

%Load the correct Bz file
[longfilename, pathname] = uigetfile('*.mat', 'Pick a magnetic field map file');
fullfilename=[pathname longfilename];
[filepath,name,ext]=fileparts(fullfilename);

load(fullfilename)

%Crop a single region for source fitting
disp(sprintf('Cropping area selection:  (+) saturate color scale, (-) desaturate color scale, \n(*) or (/) restore original color scale'));
figure;
imagesc(Bz);
title('Select area to crop')
axis equal, axis tight, axis xy
caxis([-1 1]*max(abs(caxis)));
caxis(caxis()/sqrt(10));
colormap(jet);
colorbar
flag=1;

lin=[];
col=[];
while flag
    [c,l,key]=ginput(1);
    switch key
        case 43
            caxis(caxis/sqrt(10));
        case 45
            caxis(caxis*sqrt(10));
        case {42,47}
            caxis auto, caxis([-1 1]*max(abs(caxis)));
        case 1
            lin=[lin; round(l)];
            col=[col; round(c)];
        case 27
            return
    end
    flag=length(lin)~=2;
end
lin=sort(lin,1);
col=sort(col,1);

if lin(1)<=0
    lin(1)=1;
end
if col(1)<=0
    col(1)=1;
end
if lin(2)>size(Bz,1)
    lin(2)=size(Bz,1);
end
if col(2)>size(Bz,2)
    col(2)=size(Bz,2);
end

%enforce that the cropped array has even dimensions
xrange=col(2)-col(1);
yrange=lin(2)-lin(1);
if mod(xrange,2)
    col(2,1)=col(2,1);
else
    col(2,1)=col(2,1)-1;
end
if mod(yrange,2)
    lin(2,1)=lin(2,1);
else
    lin(2,1)=lin(2,1)-1;
end

%crop map to region of interest
scanc=Bz(lin(1,1):lin(2,1),col(1,1):col(2,1));

%make array of x and y with physical units
xc=((1:size(scanc,2)))*step;
yc=((1:size(scanc,1)))*step;
[Xc,Yc]=meshgrid(xc,yc);

%iteratively fit region of interest
P00(1)=round(size(scanc,2)/2)*step;
P00(2)=round(size(scanc,1)/2)*step;
P00(3)=h;

drawnow
P=zeros(length(P00)+3,NRUNS);
fval=zeros(1,NRUNS);
fval2=zeros(1,NRUNS);
for k=1:NRUNS
    if NRUNS==1
        P0=P00; % No randomization of origin coordinats guesses if running only once
    else
        P0=P00+0.2*(rand(size(P00))-0.5).*P00; % Introduce +/-10% perturbation to initial guesses for origin coordinates
    end
    options=optimset('TolX',10^(floor(log10(m0))-5),'TolFun', 10^(floor(log10(max(abs(Bz(:)))))-8),'MaxFunEvals',6000,'MaxIter',2000,'Display','none');
    
    [P(1:3,k),fval2(k),resd,exitflag,output] = lsqnonlin(@(Pp) SourceFitDipoleBz(Pp,Xc,Yc,scanc,0,0),P0,[],[],options); %NLS method

    [resid,BzModel,M]=SourceFitDipoleBz(P(1:3,k),Xc,Yc,scanc,0,0);   % Compute field map of solution when optimization is completed
    
    Mx=M(1);
    My=M(2);
    Mz=M(3);
    m=sqrt(Mx^2+My^2+Mz^2);
    theta=acosd(Mz/m);
    phi=atan2d(My,Mx);
    P(4,k)=m;
    %convert spherical angles to inclination and declination
    P(5,k)=90-theta;
    P(6,k)=phi-90;
    
    %enforce range for angular parameters
    change=1;
    while change
        change=0;
        if P(5,k)>90
            P(5,k)=180-P(5,k); % i' = 180-i
            P(6,k)=P(6,k)+180; % d' = d+180
            change=1;
        end
        if P(5,k)<-90
            P(5,k)=-180-P(5,k); % i' = -180-i
            P(6,k)=P(6,k)+180; % d' = d+180
            change=1;
        end
        if P(6,k)<0
            P(6,k)=P(6,k)+360; % d' = d+360
            change=1;
        end
        if P(6,k)>=360
            P(6,k)=P(6,k)-360; % d' = d-360
            change=1;
        end
    end
    
    fprintf('..%0d..(%0d)  ',k,output.iterations);
    if ~mod(k,10)
        fprintf('\n');
    end
    
    fval(k)=sqrt(sum(sum((BzModel-scanc).^2))/numel(scanc)); % compute normalized residual
end

% Pick best solution as the one with minimum cost (residual). MINTOL specifies
% whether this minimum is computed based on a single point or on an average of points with
% with minimal cost. In almost all most cases, MINTOL=1 should be used.

i0=find(fval==min(fval));
fsort=sort(fval);

i=find(fval<=fsort(MINTOL));
disp(sprintf('\n--- Averaging %d points ---',numel(i)))

Popt=zeros(size(P));
Popt(1)=sum(P(1,i).*fval(i))/sum(fval(i));
Popt(2)=sum(P(2,i).*fval(i))/sum(fval(i));
Popt(3)=sum(P(3,i).*fval(i))/sum(fval(i));
Popt(4)=sum(P(4,i).*fval(i))/sum(fval(i));
Popt(5)=sum(P(5,i).*fval(i))/sum(fval(i));
Popt(6)=sum(P(6,i).*fval(i))/sum(fval(i));

Popt2=[Popt(1:4) 90-Popt(5) Popt(6)+90];
[resid,Bzmodel]=SourceFitDipoleBz(Popt2,Xc,Yc,scanc,0,0);   % Compute map with calculated optimal parameters
residex=Bzmodel-scanc;  % Compute residuals for this solution

residex=residex-mean(mean(residex));
scanc=scanc-mean(mean(scanc));

dipolarityparameter = 1 - (rms(rms(residex)) / rms(rms(scanc)));

%Show maps of original data, model, and residuals
figure
subplot(2,2,1);
imagesc(xc,yc,scanc);
axis xy, axis equal, axis tight;
caxis([-1 1]*max(abs(caxis)));
colorbar
title('Original Scan');
subplot(2,2,2);
imagesc(xc,yc,Bzmodel);
axis xy, axis equal, axis tight;
caxis([-1 1]*max(abs(caxis)));
colorbar
title('Model Scan');
subplot(2,2,4);
imagesc(xc,yc,residex);
axis xy, axis equal, axis tight;
caxis([-1 1]*max(abs(caxis)));
colorbar
title('Residuals');

disp('The dipolarity parameter for the selected region is:');
disp(dipolarityparameter);

